#include <QString>
#include "sriforcesensordll.h"
#include "Package/simpletcpsocketdll_global.h"

extern "C" SRIFORCESENSORDLLSHARED_EXPORT SRIForceSensorDLL* GetSRIForceSensorInstance(){
    return new SRIForceSensor();
}


SRIForceSensor::SRIForceSensor(){

}


SRIForceSensor::~SRIForceSensor(){

}

short SRIForceSensor::ForceSensorConnect(QString ip,int port){
    COM_addTcpClient(ip,port,this->toForceSensorSocket);
    COM_connectToServer(this->toForceSensorSocket);
    return 1;
}
short SRIForceSensor::ForceSensorDataDeal(){
    int i = 0;
    uint dataint =0;


    dataint = (uchar)RecvRawForceSensorData[9]<<24 | dataint;
    dataint = (uchar)RecvRawForceSensorData[8]<<16 | dataint;
    dataint = (uchar)RecvRawForceSensorData[7]<<8  | dataint;
    dataint = (uchar)RecvRawForceSensorData[6]     | dataint;
    this->mForceSensorData.RawData[0] =  *((float*)(&dataint));

    dataint = 0;
    dataint = (uchar)RecvRawForceSensorData[13]<<24 | dataint;
    dataint = (uchar)RecvRawForceSensorData[12]<<16 | dataint;
    dataint = (uchar)RecvRawForceSensorData[11]<<8  | dataint;
    dataint = (uchar)RecvRawForceSensorData[10]     | dataint;
    this->mForceSensorData.RawData[1] =  *((float*)(&dataint));

    dataint =0;
    dataint = (uchar)RecvRawForceSensorData[17]<<24 | dataint;
    dataint = (uchar)RecvRawForceSensorData[16]<<16 | dataint;
    dataint = (uchar)RecvRawForceSensorData[15]<<8  | dataint;
    dataint = (uchar)RecvRawForceSensorData[14]     | dataint;
    this->mForceSensorData.RawData[2] =  *((float*)(&dataint));

    dataint = 0;
    dataint = (uchar)RecvRawForceSensorData[21]<<24 | dataint;
    dataint = (uchar)RecvRawForceSensorData[20]<<16 | dataint;
    dataint = (uchar)RecvRawForceSensorData[19]<<8  | dataint;
    dataint = (uchar)RecvRawForceSensorData[18]     | dataint;
    this->mForceSensorData.RawData[3] =  *((float*)(&dataint));

    dataint = 0;
    dataint = (uchar)RecvRawForceSensorData[25]<<24 | dataint;
    dataint = (uchar)RecvRawForceSensorData[24]<<16 | dataint;
    dataint = (uchar)RecvRawForceSensorData[23]<<8  | dataint;
    dataint = (uchar)RecvRawForceSensorData[22]     | dataint;
    this->mForceSensorData.RawData[4] =  *((float*)(&dataint));

    dataint = 0;
    dataint = (uchar)RecvRawForceSensorData[29]<<24 | dataint;
    dataint = (uchar)RecvRawForceSensorData[28]<<16 | dataint;
    dataint = (uchar)RecvRawForceSensorData[27]<<8  | dataint;
    dataint = (uchar)RecvRawForceSensorData[26]     | dataint;
    this->mForceSensorData.RawData[5] =  *((float*)(&dataint));


    for(i=0; i < 6; i++){
        this->mForceSensorData.Data[i] = this->mForceSensorData.RawData[i];
    }
    return 1;
}

short SRIForceSensor::ForceSensorLoopRun(){
    if(!COM_getClientSendStatus(this->toForceSensorSocket)){
        COM_clientSendData(this->toForceSensorSocket,"AT+GOD\r\n");
    }
    if(COM_getClientReadStatus(this->toForceSensorSocket)){
        COM_clientReadData(this->toForceSensorSocket,RecvRawForceSensorData);
        this->ForceSensorDataDeal();
    }
    return 1;
}

short SRIForceSensor::ReadForceSensorData(double (&Data)[6]){
    int i = 0;
    for(i = 0; i < 6; i++){
        Data[i] = mForceSensorData.Data[i];
    }
    return 1;
}
